/*
 **********************************************************************
 * File name: cpf_att.h
 *
 * COPYRIGHT (C) 2018-2022
 *
 * The license and distribution terms for this file may be found in 
 * the LICENSE file.
 *
 * Author: ChenLong (Add new editor)
 * Email: worldlong@foxmail.com
 **********************************************************************
*/
#pragma once
/*
*/
#include "xmath/matrix/matrix.h"
#include "xmath/matrix/attitude.h"
#include "xmath/xmath.h"
/*
*/
class CPF_Att
{
public:
  /*
   */
  Quaternion q;
  bool accel_initialized;
  bool magn_initialized;
  /*
  */
  CPF_Att();
  /*
   */
  bool initialize_with_accel(Vector3f accel);
  bool initialize_with_magn(Vector3f magn);
  void predict_with_gyro(Vector3f gyro, float dt);
  void correct_with_accel(Vector3f accel, float dt);
  void correct_with_magn(Vector3f magn, float dt);
  void set_coeff(float ka, float kg, float km)
  {
    _ka = ka;
    _kg = kg;
    _km = km;
  }
  Vector3f gyro_est()  {return _w_e;}
  Vector3f gyro_bias() {return _w_b;}
  void gyro_bias_reset() {_w_b.zero();}
private:
  int _counter;
  Vector3f _accel_sum;
  Vector3f _magn_sum;
  Vector3f _w_b; //gyro bias
  Vector3f _w_e; //estimate gyro
  float _ka;
  float _km;
  float _kg;
};




